#include "Matrix.h"

/// <summary>
/// Default Constructor
/// </summary>
CICore::Matrix::Matrix() {
    LoadIdentity();
}

/// <summary>
/// Constructor
/// </summary>
CICore::Matrix::Matrix (CICore::Matrix& m) {
    unsigned short i;

    for (unsigned short r = 0; r < 4; r++)
        for (unsigned short c = 0; c < 4; c++) {
            i = (r<<2) + c;
            elements[i] = m.elements[i];        //Assigns the value
        }

}

/// <summary>
/// Destructor
/// </summary>
CICore::Matrix::~Matrix() {}









///<************************************************************************************************************>
///<**************************************** region Utility Methods ********************************************>
///<************************************************************************************************************>

/// <summary>
/// Method called to compute and get the 3x3 matrix's determinant
/// </summary>
/// <returns> The 3x3 matrix's determinant </returns>
float CICore::Matrix::GetDeterminant3() const
{
   float det =
        elements[0] * (elements[5]*elements[10] - elements[6]*elements[9])
      - elements[4] * (elements[1]*elements[10] - elements[2]*elements[9])
      + elements[8] * (elements[1]*elements[6]  - elements[2]*elements[5]);

   return det;
}

/// <summary>
/// Method called to compute and get the matrix's determinant
/// </summary>
/// <returns> The matrix's determinant </returns>
float CICore::Matrix::GetDeterminant() const {
    float result = 0;

    for (int n = 0, i = 1; n < 4; n++, i *= -1) {
        Matrix msub3;
        msub3 = GetSubMatrix (0, n);

        float det = msub3.GetDeterminant3();
        result += elements[n] * det * i;
    }

    return result;
}

/// <summary>
/// Method called to get the submatrix i x j
/// </summary>
/// <returns> The submatrix i x j </returns>
CICore::Matrix CICore::Matrix::GetSubMatrix (int i, int j) const {
    Matrix subm;

    for( int di = 0; di < 3; di ++ )
        for( int dj = 0; dj < 3; dj ++ ) {
            int si = di + (( di >= i ) ? 1 : 0);
            int sj = dj + (( dj >= j ) ? 1 : 0);

            subm.SetValue (di, dj, elements[si * 4 + sj]);
        }

    return subm;
}

/// <summary>
/// Method called to compute and get the matrix's inverse
/// </summary>
/// <returns> The matrix's inverse </returns>
CICore::Matrix CICore::Matrix::GetInverse() const {
    Matrix inverse;

    float det = GetDeterminant();

    if (det == 0) //(fabs (det) < 0.0005)            //TODO -> What's better???
        return inverse;

    for (unsigned short i = 0; i < 4; i++)
        for (unsigned short j = 0; j < 4; j++) {
            int sign = 1 - ((i +j) % 2) * 2;
            Matrix mtemp;
            mtemp = GetSubMatrix (i, j);
            inverse.SetValue (j, i, (mtemp.GetDeterminant3() * sign ) / det);
        }

    return inverse;
}

/// <summary>
/// Method called to get the value identified by row and col
/// </summary>
/// <param name="row"> The row of the element to get </param>
/// <param name="col"> The column of the element to get </param>
/// <returns> The value of the element identified by row and col </returns>
float CICore::Matrix::GetValue (unsigned int row, unsigned int col) const {
    return elements[(row<<2) + col];
}

/// <summary>
/// Method called to make this matrix the identity matrix
/// </summary>
void CICore::Matrix::LoadIdentity () {
    for (unsigned short r = 0; r < 4; r++)
        for (unsigned short c = 0; c < 4; c++)
            elements[(r<<2) + c] = (r == c) ? 1.0f : 0;        //Assigns the value depending of the element is on the matrix's diagonal or not
}

/// <summary>
/// Method called to set the value identified by row and col
/// </summary>
/// <param name="row"> The row of the element to modify </param>
/// <param name="col"> The column of the element to modify </param>
/// <param name="row"> The new element value </param>
void CICore::Matrix::SetValue (unsigned int row, unsigned int col, const float value) {
    elements[(row<<2) + col] = value;        //Assigns the new value
}

/// <summary>
/// Method called to set the translation required
/// </summary>
/// <param name="v"> The vector containing the translation values </param>
void CICore::Matrix::SetTranslation (const float* v) {
    elements[12] = v[0];        //Assigns the x-axis translation value
    elements[13] = v[1];        //Assigns the y-axis translation value
    elements[14] = v[2];        //Assigns the z-axis translation value
}

/// <summary>
/// Method called to set the translation required
/// </summary>
/// <param name="x"> The x-axis translation value </param>
/// <param name="y"> The y-axis translation value </param>
/// <param name="z"> The z-axis translation value </param>
void CICore::Matrix::SetTranslation (const float x, const float y, const float z) {
    elements[12] = x;        //Assigns the x-axis translation value
    elements[13] = y;        //Assigns the y-axis translation value
    elements[14] = z;        //Assigns the z-axis translation value
}

/// <summary>
/// Method called to set the rotation required in radians
/// </summary>
/// <param name="radians"> The vector containing the rotation values in radians </param>
void CICore::Matrix::SetRotationInRadians (const float* radians) {
    float cr = cos (radians[0]);
    float sr = sin (radians[0]);
    float cp = cos (radians[1]);
    float sp = sin (radians[1]);
    float cy = cos (radians[2]);
    float sy = sin (radians[2]);
    float srsp = sr * sp;
    float crsp = cr * sp;

    elements[0]  = cp * cy;
    elements[1]  = cp * sy;
    elements[2]  = -sp;

    elements[4]  = (srsp * cy) - (cr * sy);
    elements[5]  = (srsp * sy) + (cr * cy);
    elements[6]  = (sr * cp);

    elements[8]  = (crsp * cy) + (sr * sy);
    elements[9]  = (crsp * sy) - (sr * cy);
    elements[10] = (cr * cp);
}

/// <summary>
/// Method called to set the rotation required in degrees
/// </summary>
/// <param name="degrees"> The vector containing the rotation values in degrees </param>
void CICore::Matrix::SetRotation (const float* degrees) {
    float radians[3];

    //Computing degrees to radians conversion
    radians[0] = degrees[0] * CICore::PI_OVER_180;
    radians[1] = degrees[1] * CICore::PI_OVER_180;
    radians[2] = degrees[2] * CICore::PI_OVER_180;

    SetRotationInRadians (radians);
}

/// <summary>
/// Method called to set the rotation required in degrees
/// </summary>
/// <param name="x"> The x-axis rotation value in degrees </param>
/// <param name="y"> The y-axis rotation value in degrees </param>
/// <param name="z"> The z-axis rotation value in degrees </param>
void CICore::Matrix::SetRotation (const float x, const float y, const float z) {
    float radians[3];

    //Computing degrees to radians conversion
    radians[0] = x * CICore::PI_OVER_180;
    radians[1] = y * CICore::PI_OVER_180;
    radians[2] = z * CICore::PI_OVER_180;

    SetRotationInRadians (radians);
}

/// <summary>
/// Method called to set the uniform scaling required
/// </summary>
/// <param name="value"> The scaling value </param>
void CICore::Matrix::SetScaling (const float value) {
    elements[ 0] = value;        //Assigns the x-axis translation value
    elements[ 5] = value;        //Assigns the y-axis translation value
    elements[10] = value;        //Assigns the z-axis translation value
}

/// <summary>
/// Method called to set the scaling required
/// </summary>
/// <param name="x"> The x-axis scaling value </param>
/// <param name="y"> The y-axis scaling value </param>
/// <param name="z"> The z-axis scaling value </param>
void CICore::Matrix::SetScaling (const float x, const float y, const float z) {
    elements[ 0] = x;        //Assigns the x-axis translation value
    elements[ 5] = y;        //Assigns the y-axis translation value
    elements[10] = z;        //Assigns the z-axis translation value
}


/// <summary>
/// Method called to represent the matrix in a string
/// </summary>
/// <returns> The string representation </returns>
std::string CICore::Matrix::ToString() const {
    std::stringstream ss (std::stringstream::in | std::stringstream::out);;

    for (unsigned short  r = 0; r < 4; r++) {
        for (unsigned short  c = 0; c < 4; c++)
            ss << elements[(r<<2) + c] << " ";
        ss << "\n";
    }
    return ss.str();
}

/// <summary>
/// Assignment 
/// </summary>
/// <param name="b"> The matrix to be copied </param>
/// <returns> A reference of the matrix modified </returns>
CICore::Matrix& CICore::Matrix::operator = (const Matrix& m) {
    unsigned short i;

    for (unsigned short r = 0; r < 4; r++)
        for (unsigned short c = 0; c < 4; c++) {
            i = (r<<2) + c;
            elements[i] = m.elements[i];        //Assigns the value
        }
    return *this;
}
/***************************************************************************************************************
******************************************* endregion Utility Methods ******************************************
****************************************************************************************************************/









///<************************************************************************************************************>
///<******************************* region BASIC MATRIX's BINARY OPERATIONS ************************************>
///<************************************************************************************************************>

/// <summary>
/// Sum between two matrices
/// </summary>
/// <param name="a"> The first matrix </param>
/// <param name="b"> The second matrix </param>
/// <returns> The sum between the two matrices </returns>
CICore::Matrix CICore::operator + (const Matrix& a, const Matrix& b) {
    Matrix m;

    for (unsigned short r = 0; r < 4; r++)
        for (unsigned short c = 0; c < 4; c++)
            m.elements[(r<<2) + c]
                = a.elements[(r<<2) + 0] + b.elements[(r<<2) + 0]
                + a.elements[(r<<2) + 1] + b.elements[(r<<2) + 1]
                + a.elements[(r<<2) + 2] + b.elements[(r<<2) + 2]
                + a.elements[(r<<2) + 3] + b.elements[(r<<2) + 3]
                ;

    return m;
}

/// <summary>
/// Difference between two matrices
/// </summary>
/// <param name="a"> The first matrix </param>
/// <param name="b"> The second matrix </param>
/// <returns> The difference between the two matrices </returns>
CICore::Matrix CICore::operator - (const Matrix& a, const Matrix& b) {
    Matrix m;

    for (unsigned short r = 0; r < 4; r++)
        for (unsigned short c = 0; c < 4; c++)
            m.elements[(r<<2) + c]
                = a.elements[(r<<2) + 0] - b.elements[(r<<2) + 0]
                + a.elements[(r<<2) + 1] - b.elements[(r<<2) + 1]
                + a.elements[(r<<2) + 2] - b.elements[(r<<2) + 2]
                + a.elements[(r<<2) + 3] - b.elements[(r<<2) + 3]
                ;

    return m;
}

/// <summary>
/// Product between two matrices
/// </summary>
/// <param name="a"> The multiplicand matrix </param>
/// <param name="b"> The multiplier matrix </param>
/// <returns> The product between the two matrices </returns>
CICore::Matrix CICore::operator * (const Matrix& a, const Matrix& b) {
    Matrix m;

    for (unsigned short r = 0; r < 4; r++)
        for (unsigned short c = 0; c < 4; c++)
            m.elements[(r<<2) + c]
                = a.elements[(r<<2) + 0] * b.elements[(0<<2) + c]
                + a.elements[(r<<2) + 1] * b.elements[(1<<2) + c]
                + a.elements[(r<<2) + 2] * b.elements[(2<<2) + c]
                + a.elements[(r<<2) + 3] * b.elements[(3<<2) + c]
                ;

    return m;
}

///// <summary>
///// Assignment by multiplication between two matrices
///// </summary>
///// <param name="a"> The matrix to be modified </param>
///// <param name="b"> The multiplier matrix </param>
///// <returns> A reference of the matrix modified </returns>
//CICore::Matrix& CICore::operator *= (Matrix& a, const Matrix& b) {
//    Matrix m;
//
//    for (int r = 0; r < 4; r++)
//        for (int c = 0; c < 4; c++)
//            m.elements[(r<<2) + c]
//                = a.elements[(r<<2) + 0] * b.elements[(0<<2) + c]
//                + a.elements[(r<<2) + 1] * b.elements[(1<<2) + c]
//                + a.elements[(r<<2) + 2] * b.elements[(2<<2) + c]
//                + a.elements[(r<<2) + 3] * b.elements[(3<<2) + c]
//                ;
//
//    a.elements = m.elements;
//
//    return a;
//}
/***************************************************************************************************************
********************************** endregion BASIC MATRIX's BINARY OPERATIONS **********************************
****************************************************************************************************************/
